#include "modules/drivers/lidar/orin_hesai/hesai_utils/hesai_parser.h"

#include <time.h>

#include <cmath>
#include <new>

namespace apollo {
namespace drivers {
namespace lidar_hesai {
bool HesaiParser::Init() {
  InitHoriCosSinTable();
  InitVertCosSinTable();
  InitBlockOffset();
  InitLaserOffset();
  AINFO << "HesaiParser init successful.";
  return true;
}

bool HesaiParser::ParseData(const HesaiLidarPacketData* data, PointCloud* cloud,
                            uint32_t& startAzimuth, uint32_t& endAzimuth) {
  if (data == nullptr || cloud == nullptr) {
    AERROR << "Invalid input!";
    return false;
  }
  cloud->mutable_point()->Reserve(UNIT_PER_BLOCK * 2200);
  if (!Unpack(*data, packet_buffer_)) {
    AERROR << "Error when unpacking raw data!";
    return false;
  }
  if (!CalcPointCloud(packet_buffer_, cloud, startAzimuth, endAzimuth)) {
    AERROR << "Error when calculating point cloud!";
    return false;
  }
  return true;
}

int HesaiParser::AppendZeroPacketByAzimuthGap(const uint32_t azimuthGap,
                                              PointCloud* cloud, bool flag) {
  int missedPacket = round(azimuthGap / AZIMUTH_PER_PACKET);
  AINFO << "AppendZeroPacketByAzimuthGap gap:" << azimuthGap
        << " misspkt: " << missedPacket;
  if (missedPacket > 0) {
    AINFO << "Packet missed, azimuth gap=" << azimuthGap << ", " << missedPacket
          << " packet(s) of zeros are appended.";
    AppendZeroPoint(missedPacket * POINT_PER_PACKET, cloud);
  }
  return missedPacket;
}

void HesaiParser::AppendZeroPoint(const uint32_t num, PointCloud* cloud) {
  for (int i = 0; i < num; i++) {
    PointXYZIT* pt = cloud->add_point();
    pt->set_x(NAN);
    pt->set_y(NAN);
    pt->set_z(NAN);
    pt->set_intensity(0);
    pt->set_timestamp(0);
  }
}

bool HesaiParser::Unpack(const HesaiLidarPacketData& buf,
                         HS_LIDAR_XT_Packet& packet) {
  // parse header
  int index = 0;
  packet.header.sob = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff));
  if (packet.header.sob != 0xEEFF) {
    AERROR << "Error start of packet!";
    return false;
  }
  packet.header.chProtocolMajor = buf[index + 2] & 0xff;
  packet.header.chProtocolMinor = buf[index + 3] & 0xff;
  packet.header.chLaserNumber = buf[index + 6] & 0xff;
  packet.header.chBlockNumber = buf[index + 7] & 0xff;
  if ((int)packet.header.chBlockNumber != BLOCK_NUM) {
    AERROR << "Error packet data: bad block number!";
    return false;
  }
  packet.header.chReturnType = buf[index + 8] & 0xff;
  packet.header.chDisUnit = buf[index + 9] & 0xff;

  // parse main data
  index = MAIN_DATA_IDX;
  for (int block = 0; block < packet.header.chBlockNumber; block++) {
    packet.blocks[block].azimuth =
        (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8);
    index += 2;
    for (int unit = 0; unit < packet.header.chLaserNumber; unit++) {
      uint32_t range = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8);
      packet.blocks[block].units[unit].distance =
          static_cast<double>(range * packet.header.chDisUnit) / 1000.0;
      packet.blocks[block].units[unit].intensity = (buf[index + 2] & 0xff);
      packet.blocks[block].units[unit].confidence = (buf[index + 3] & 0xff);
      index += 4;
    }
  }

  // parse tail
  packet.echo = buf[index + 10] & 0xff;
  packet.utc_time[0] = buf[index + 13] & 0xff;
  packet.utc_time[1] = buf[index + 14] & 0xff;
  packet.utc_time[2] = buf[index + 15] & 0xff;
  packet.utc_time[3] = buf[index + 16] & 0xff;
  packet.utc_time[4] = buf[index + 17] & 0xff;
  packet.utc_time[5] = buf[index + 18] & 0xff;
  packet.timestamp = (buf[index + 19] & 0xff) | (buf[index + 20] & 0xff) << 8 |
                     (buf[index + 21] & 0xff) << 16 |
                     (buf[index + 22] & 0xff) << 24;
  return true;
}

bool HesaiParser::CalcPointCloud(HS_LIDAR_XT_Packet& packet, PointCloud* cloud,
                                 uint32_t& startAzimuth, uint32_t& endAzimuth) {
  for (int i = 0; i < packet.header.chBlockNumber; ++i) {
    if (i == 0) {
      startAzimuth = packet.blocks[i].azimuth;
    }
    if (i == packet.header.chBlockNumber - 1) {
      endAzimuth = packet.blocks[i].azimuth;  // + AZIMUTH_PER_BLOCK;
    }
    CalcPointXYZIT(packet, i, cloud);
  }
  return true;
}

void HesaiParser::CalcPointXYZIT(HS_LIDAR_XT_Packet& pkt, int blockid,
                                 PointCloud* cloud) {
  HS_LIDAR_XT_Block& block = pkt.blocks[blockid];
  struct tm tTm;
  // UTC's year only include 0 - 99 year , which indicate 2000 to 2099.
  // and mktime's year start from 1900 which is 0. so we need add 100 year.
  tTm.tm_year = pkt.utc_time[0] + 100;
  if (tTm.tm_year >= 200) {
    tTm.tm_year -= 100;
  }
  // UTC's month start from 1, but mktime only accept month from 0.
  tTm.tm_mon = pkt.utc_time[1] - 1;
  tTm.tm_mday = pkt.utc_time[2];
  tTm.tm_hour = pkt.utc_time[3];
  tTm.tm_min = pkt.utc_time[4];
  tTm.tm_sec = pkt.utc_time[5];
  tTm.tm_isdst = 0;

  double unix_second = static_cast<double>(make_time_->mktime_kernel(&tTm));
  double timestamp =
      unix_second + (static_cast<double>(pkt.timestamp)) / 1000000.0;

  for (int i = 0; i < UNIT_PER_BLOCK; i++) {
    HS_LIDAR_XT_Unit& unit = block.units[i];
    float x, y, z, intensity;
    if (unit.distance < 0.1 || unit.distance > 200.0) {
      x = y = z = intensity = 0.0;
    } else {
      double xyDistance = unit.distance * cos_vert_table_[i];
      x = -static_cast<float>(xyDistance * cos_hori_table_[block.azimuth][i]);
      y = static_cast<float>(xyDistance * sin_hori_table_[block.azimuth][i]);
      z = static_cast<float>(unit.distance * sin_vert_table_[i]);
      intensity = unit.intensity;
    }
    PointXYZIT* new_point = cloud->add_point();
    new_point->set_x(x);
    new_point->set_y(y);
    new_point->set_z(z);
    new_point->set_intensity(intensity);
    double point_timestamp = timestamp;
    if (pkt.echo == 0x39) {
      point_timestamp +=
          (static_cast<double>(block_offset_[blockid / 2] + laser_offset_[i]) /
           1000000.0f);
    } else {
      point_timestamp +=
          (static_cast<double>(block_offset_[blockid] + laser_offset_[i]) /
           1000000.0f);
    }
    uint64_t stamp = static_cast<uint64_t>(point_timestamp * 1e9);
    new_point->set_timestamp(stamp);
  }
}

void HesaiParser::InitHoriCosSinTable() {
  for (uint32_t i = 0; i < 36001; ++i) {
    for (uint32_t j = 0; j < UNIT_PER_BLOCK; ++j) {
      double arg_hori = DegreeToRadian((static_cast<double>(i)) * 0.01);
      sin_hori_table_[i][j] = sinf(arg_hori);
      cos_hori_table_[i][j] = cosf(arg_hori);
    }
  }
}
void HesaiParser::InitVertCosSinTable() {
  for (uint32_t i = 0; i < UNIT_PER_BLOCK; ++i) {
    float arg_vert = DegreeToRadian(elevAngleMap[i]);
    sin_vert_table_[i] = sinf(arg_vert);
    cos_vert_table_[i] = cosf(arg_vert);
  }
}

void HesaiParser::InitBlockOffset() {
  // init the block time offset, us
  block_offset_[0] = 3.28 - 50.0 * 7.0;
  block_offset_[1] = 3.28 - 50.0 * 6.0;
  block_offset_[2] = 3.28 - 50.0 * 5.0;
  block_offset_[3] = 3.28 - 50.0 * 4.0;
  block_offset_[4] = 3.28 - 50.0 * 3.0;
  block_offset_[5] = 3.28 - 50.0 * 2.0;
  block_offset_[6] = 3.28 - 50.0 * 1.0;
  block_offset_[7] = 3.28 - 50.0 * 0.0;
}

void HesaiParser::InitLaserOffset() {
  // init the laser shot time offset, us
  laser_offset_[0] = 0.28 + 1.512 * 0.0;
  laser_offset_[1] = 0.28 + 1.512 * 1.0;
  laser_offset_[2] = 0.28 + 1.512 * 2.0;
  laser_offset_[3] = 0.28 + 1.512 * 3.0;
  laser_offset_[4] = 0.28 + 1.512 * 4.0;
  laser_offset_[5] = 0.28 + 1.512 * 5.0;
  laser_offset_[6] = 0.28 + 1.512 * 6.0;
  laser_offset_[7] = 0.28 + 1.512 * 7.0;
  laser_offset_[8] = 0.28 + 1.512 * 8.0;
  laser_offset_[9] = 0.28 + 1.512 * 9.0;
  laser_offset_[10] = 0.28 + 1.512 * 10.0;
  laser_offset_[11] = 0.28 + 1.512 * 11.0;
  laser_offset_[12] = 0.28 + 1.512 * 12.0;
  laser_offset_[13] = 0.28 + 1.512 * 13.0;
  laser_offset_[14] = 0.28 + 1.512 * 14.0;
  laser_offset_[15] = 0.28 + 1.512 * 15.0;

  laser_offset_[16] = 0.28 + 1.512 * 16.0;
  laser_offset_[17] = 0.28 + 1.512 * 17.0;
  laser_offset_[18] = 0.28 + 1.512 * 18.0;
  laser_offset_[19] = 0.28 + 1.512 * 19.0;
  laser_offset_[20] = 0.28 + 1.512 * 20.0;
  laser_offset_[21] = 0.28 + 1.512 * 21.0;
  laser_offset_[22] = 0.28 + 1.512 * 22.0;
  laser_offset_[23] = 0.28 + 1.512 * 23.0;
  laser_offset_[24] = 0.28 + 1.512 * 24.0;
  laser_offset_[25] = 0.28 + 1.512 * 25.0;
  laser_offset_[26] = 0.28 + 1.512 * 26.0;
  laser_offset_[27] = 0.28 + 1.512 * 27.0;
  laser_offset_[28] = 0.28 + 1.512 * 28.0;
  laser_offset_[29] = 0.28 + 1.512 * 29.0;
  laser_offset_[30] = 0.28 + 1.512 * 30.0;
  laser_offset_[31] = 0.28 + 1.512 * 31.0;
}
}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo